#include "ubt_com/ubt_com_util.h"
#include "ubt_com/ubt_com_msg_head_api.h"
#include "ubt_state/ubt_state_util.h"
#include "ubt_state/ubt_state_task_adjust.h"
#include "ubt_state/node.h"

namespace ubt_state
{
struct AdjustClsData
{
    rclcpp::Publisher<ubt_interface::msg::UbtAdjustMsg>::SharedPtr _pub_adjust;
    rclcpp::Subscription<ubt_interface::msg::UbtAdjustMsg>::SharedPtr _sub_adjust;
    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _pub_v_recharge;
    rclcpp::Subscription<std_msgs::msg::Int64 >::SharedPtr _sub_v_recharge;
    CTaskAdjust *_pCurTask;
};
static AdjustClsData g_clsData;

void Sub_Ajust_CallbackWrap(const ubt_interface::msg::UbtAdjustMsg::SharedPtr msg)
{
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Sub_Adjust_Callback(msg);
    }
}

void Sub_V_Recharge_CallbackWrap(const std_msgs::msg::Int64::SharedPtr msg)
{
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Sub_V_Recharge_Callback(msg);
    }
}

int CTaskAdjust::ClassInit()
{
    AdjustClsData *pClsData = &g_clsData;
    pClsData->_pub_adjust = node->create_publisher<ubt_interface::msg::UbtAdjustMsg>("/ubt_adjust_in_v1", 1);
    pClsData->_sub_adjust = node->create_subscription<ubt_interface::msg::UbtAdjustMsg>("/ubt_adjust_out_v1", 10, std::bind(&Sub_Ajust_CallbackWrap, std::placeholders::_1));
    pClsData->_pub_v_recharge = node->create_publisher<std_msgs::msg::String>("/recharge/ctrl", 1);
    pClsData->_sub_v_recharge = node->create_subscription<std_msgs::msg::Int64 >("/recharge_result", 10, std::bind(&Sub_V_Recharge_CallbackWrap, std::placeholders::_1));
    
    pClsData->_pCurTask = NULL;
    return 0;
}

void CTaskAdjust::ClassCleanup()
{
    AdjustClsData *pClsData = &g_clsData;
    // pClsData->_pub_adjust->Shutdown();
    // pClsData->_sub_adjust->Shutdown();
    // pClsData->_pub_v_recharge->Shutdown();
    // pClsData->_sub_v_recharge->Shutdown();
    pClsData->_pCurTask = NULL;
}

ITask *CTaskAdjust::CreateTask(ITaskCb *pTaskCb, std::string taskId, ubt_task_stage stage, int timeOutMs,
                               ubt_proto_adjust::ubt_adjust_pose_type poseType, geometry_msgs::msg::Pose2D poseDst, geometry_msgs::msg::Pose2D poseLmt,
                               float stopDis, float sideDis, int avoidHwMask,
                               int index, int reportIndex, std::string actionType)

{
    CTaskAdjust *pTask = new CTaskAdjust();
    if (!pTask->Init(pTaskCb, taskId, stage, timeOutMs,
                     poseType, poseDst, poseLmt,
                     stopDis, sideDis, avoidHwMask, index, reportIndex, actionType))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

// CreateTask 的重载，用于U1000 uslam回充
ITask *CTaskAdjust::CreateTask(ITaskCb *pTaskCb, std::string taskId, ubt_task_stage stage, bool isRechargeAdjust, int index, int reportIndex)
{
    CTaskAdjust *pTask = new CTaskAdjust();
    if (!pTask->Init(pTaskCb, taskId, stage, isRechargeAdjust, index, reportIndex))
    {
        delete pTask;
        pTask = NULL;
    }
    return (ITask *)pTask;
}

CTaskAdjust::~CTaskAdjust()
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s index=%d _state=%d poseType=%d reportIndex=%d",
		_taskId.c_str(),_index, _state,_poseType,_reportIndex);
    if ((_state != ubt_task_state_idle) && (_state != ubt_task_state_closed))
    {
        _SetState(ubt_task_state_closed, ubt_task_result_adjust_base, "destroy task wrong");
    }
}

bool CTaskAdjust::Init(ITaskCb *pTaskCb, std::string taskId,  ubt_task_stage stage, int timeOutMs,
                       ubt_proto_adjust::ubt_adjust_pose_type poseType, geometry_msgs::msg::Pose2D poseDst, geometry_msgs::msg::Pose2D poseLmt,
                       float stopDis, float sideDis, int avoidHwMask,
                       int index, int reportIndex, std::string actionType)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s index=%d stage=%d stopDis=%f sideDis=%f avoidHwMask=%d",
		taskId.c_str(),index, stage, stopDis, sideDis, avoidHwMask);
    _pTaskCb = pTaskCb;
    _taskId = taskId;
    _index = index;
    _reportIndex = reportIndex;
    _actionType = actionType;
    _stage = stage;
    _timeOutMs = timeOutMs;
    _poseType = poseType;
    _poseDst = poseDst;
    _poseLmt = poseLmt;
    _stopDis = stopDis;
    _sideDis = sideDis;
    _avoidHwMask = avoidHwMask;

    _state = ubt_task_state_idle;
    _stateTime = ubt_com::GetNowInMsecs();
    _msgId = 0;
    _avoidHwStatus = 0;
    _avoidDisable = 0;
    _pauseState = 0;
    _stopReason = 0;

    return true;
}

bool CTaskAdjust::Init(ITaskCb *pTaskCb, std::string taskId,  ubt_task_stage stage, bool isRechargeAdjust, int index, int reportIndex)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] Init : taskId=%s, stage=%d, isRechargeAdjust=%d, index=%d, reportIndex=%d", 
        taskId.c_str(), stage, isRechargeAdjust, index, reportIndex);
    _pTaskCb = pTaskCb;
    _taskId = taskId;
    _stage = stage;
    _isRechargeAdjust = isRechargeAdjust;
    _index = index;
    _reportIndex = reportIndex;

    _state = ubt_task_state_idle;
    _stateTime = ubt_com::GetNowInMsecs();
    _msgId = 0;
    _avoidHwStatus = 0;
    _avoidDisable = 0;
    _pauseState = 0;
    _stopReason = 0;

    return true;
}

ubt_task_type CTaskAdjust::GetType()
{
    return ubt_task_type_adjust;
}

bool CTaskAdjust::Start()
{
    
    int ires;
    RCLCPP_INFO(node->get_logger(),"taskId=%s _state=%d", _taskId.c_str(), _state);
    if (g_clsData._pCurTask != NULL)
    {
        RCLCPP_WARN(node->get_logger(),"fail curTask is not null,taskId=%s ", _taskId.c_str());
        return false;
    }
    if (_state != ubt_task_state_idle)
    {
        RCLCPP_WARN(node->get_logger(),"fail taskId=%s state=%d is not idle", _taskId.c_str(), _state);
        return false;
    }
    if (ubt_com::isZero(_stopDis))
    {
        _avoidDisable = 1;
    }
    ObstacleParams    obsParams;
    obsParams.stopDis = _stopDis;
    obsParams.slowDis = 0.3;
    obsParams.warnDis = 0.5;
    obsParams.sideDis = _sideDis;
    UbtStateUtilSetObstacleParams(obsParams);
    _OnLocationCb();
    _OnAvoidStatusCb();
    _CheckPause(0);
    _SetState(ubt_task_state_running, 0, "start cmd");
    if (! _isRechargeAdjust)
    {
        ubt_proto_adjust::ubt_adjust_msg_start_req startReq;
        startReq.taskId = _taskId;
        startReq.lifeTime = 5 * 60;
        startReq.poseType = _poseType;
        startReq.poseDst = _poseDst;
        startReq.poseLmt = _poseLmt;
        startReq.isPause = _pauseState;
        _SendMsg(ubt_proto_adjust::ubt_adjust_msg_type_start_req, &startReq);
    }
    else
    {
        _SendMsg(0, "request_recharge", 0);
    }

    _pTaskCb->NotifyTaskStateAndStage(_state, _stage, 0, "");
    
    return true;
}

void CTaskAdjust::Pause()
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s _state=%d", _taskId.c_str(), _state);
    if (_state == ubt_task_state_running)
    {
        _SetState(ubt_task_state_pause, 0, "pause cmd");
        _CheckPause(2);
    }
}

void CTaskAdjust::Continue()
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s _state=%d", _taskId.c_str(), _state);
    if (_state == ubt_task_state_pause)
    {
        _SetState(ubt_task_state_running, 0, "continue cmd");
        _CheckPause(2);
    }
}

void CTaskAdjust::Stop(int reason)
{
    _Stop(reason);
}
void CTaskAdjust::_Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] _Stop : ==================== start ====================");
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] _Stop : taskId=%s _state=%d reason=%d",
             _taskId.c_str(), _state, reason);
    if (_state >= ubt_task_state_stopping) // ubt_task_state_stopping or ubt_task_state_closed
    {
        return;
    }
    _stopReason = reason;
    _stopReasonDesc = UbtStateGetTaskResultDesc(_stopReason);
    if (_state == ubt_task_state_idle)
    {
        _SetState(ubt_task_state_closed, reason, "stop cmd");
        _pTaskCb->NotifyTaskStateAndStage(_state, _stage, reason, "");
        return;
    }
    if (! _isRechargeAdjust)
    {
        ubt_proto_adjust::ubt_adjust_msg_stop_req stopReq;
        stopReq.iCleanInfo = 0;
        stopReq.duration = 0;
        switch (reason)
        {
            case ubt_task_result_usr_stopbtn:
                stopReq.runStatus = ubt_proto_adjust::ubt_adjust_run_status_stopbtn;
                break;
            case ubt_task_result_sys_nrfault:
                stopReq.runStatus = ubt_proto_adjust::ubt_adjust_run_status_nrfault;
            case ubt_task_result_adjust_timeout:
                stopReq.runStatus = ubt_proto_adjust::ubt_adjust_run_status_timeout;
            default:
                stopReq.runStatus = ubt_proto_adjust::ubt_adjust_run_status_cancel;
        }
        _SendMsg(ubt_proto_adjust::ubt_adjust_msg_type_stop_req, &stopReq);
        _SetState(ubt_task_state_stopping, 0, "stop cmd");
    }
    else if (_isRechargeAdjust &&
        (_stopReason == ubt_task_result_usr_stopbtn || 
        _stopReason == ubt_task_result_usr_brakebtn ||
        _stopReason == ubt_task_result_sys_antc ||
        _stopReason == ubt_task_result_usr_cancel ||
        _stopReason == ubt_task_result_sys_nrfault ||
        _stopReason == ubt_task_result_navigation_cancel))
    {
        _SetState(ubt_task_state_stopping, 0, "stop cmd");
        _SendMsg(1, "request_recharge", 1);
        _SetState(ubt_task_state_closed, ubt_task_result_adjust_base, "");  // uslam recharge, direct close.
    }

    _pTaskCb->NotifyTaskStateAndStage(_state, _stage, 0, "");
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] _Stop : ==================== end ====================");
    return;
}

ubt_task_state CTaskAdjust::GetState()
{
    ubt_task_state state = _state;
    if (state == ubt_task_state_running)
    {
        if (_pauseState == 1)
        {
            state = ubt_task_state_obstacle;
        }
    }
    return state;
}

ubt_task_stage CTaskAdjust::GetStage()
{
    return _stage;
}

std::string CTaskAdjust::GetActionType()
{
    return _actionType;
}

void CTaskAdjust::GetIndex(int& index, int& reportIndex)
{
    index = _index;
    reportIndex = _reportIndex;
}

void CTaskAdjust::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    stopReason = _stopReason;
    stopReasonDesc = _stopReasonDesc;
}

void CTaskAdjust::GetLogs(std::vector<TaskLogItem>& logs)
{
}

void CTaskAdjust::OnTimer()
{
    if (_state == ubt_task_state_running)
    {
        if ((ubt_com::GetNowInMsecs() - _stateTime) > _timeOutMs)
        {
            _Stop(ubt_task_result_adjust_timeout);
        }
    }
}
void CTaskAdjust::_SetState(ubt_task_state newState, int reason, std::string reasonStr)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s newState=%d oldState=%d reason=%d reasonStr=%s",
             _taskId.c_str(), newState, _state, reason, reasonStr.c_str());
    if (newState == _state)
    {
        return;
    }
    int oldState = _state;
    _state = newState;
    _stateTime = ubt_com::GetNowInMsecs();
    if (_state == ubt_task_state_running)
    {
        if (oldState == ubt_task_state_idle)
        {
            g_clsData._pCurTask = this;
        }
    }
    if (_state == ubt_task_state_closed)
    {
        _stopReason = reason;
        _stopReasonDesc = reasonStr;
        if (g_clsData._pCurTask == this)
        {
            g_clsData._pCurTask = NULL;
        }
    }
}

void CTaskAdjust::OnMsg(int msgType, void *pvMsg)
{
    RCLCPP_INFO(node->get_logger(),"CTaskAdjust::OnMsg msgType: %d",msgType);
    do
    {
        if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
        {
            break;
        }
        if (msgType == ubt_msg_type_current_pose)
        {
            _OnLocationCb();
            _CheckPause(2);
            break;
        }
        if (msgType == ubt_msg_type_avoid_status)
        {
            _OnAvoidStatusCb();
            _CheckPause(1);
            break;
        }
    }
    while (false);
}

void CTaskAdjust::_OnLocationCb()
{
}

void  CTaskAdjust::_OnAvoidStatusCb()
{
    StateStatus *pStatus = _pTaskCb->GetStateStatus();
    if (_avoidHwMask & avoid_hw_mask_rgbd)
    {
        int laserAvoidStatus = pStatus->laserAvoidStatus;
        if ((laserAvoidStatus == avoid_status_value_stopleft) ||
                (laserAvoidStatus == avoid_status_value_stopright))
        {
            laserAvoidStatus = avoid_status_value_stop;
        }
        _avoidHwStatus = laserAvoidStatus;
    }
    else
    {
        _avoidHwStatus = pStatus->avoidHwStatus;
    }
}

void CTaskAdjust::_CheckPause(int event)
{
    int oldPauseState = _pauseState;
    _pauseState = 0;
    if (((_avoidHwStatus == 1) && (_avoidDisable == 0)) ||
            (_state == ubt_task_state_pause))
    {
        _pauseState = 1;
    }
    if (event == 0)
    {
        return;
    }
    if (oldPauseState != _pauseState)
    {
        if (! _isRechargeAdjust)
        {
            int msgType;
            if (_pauseState != 0)
            {
                msgType = ubt_proto_adjust::ubt_adjust_msg_type_pause_req;
            }
            else
            {
                msgType = ubt_proto_adjust::ubt_adjust_msg_type_continue_req;
            }
            RCLCPP_INFO(node->get_logger(),"taskId=%s _pauseState=%d event=%d oldPauseState=%d",
                    _taskId.c_str(), _pauseState, event, oldPauseState);
            _SendMsg(msgType, NULL);
        }
        else if (_pauseState == 1 && _state == ubt_task_state_pause)
        {
            _SendMsg(1, "request_recharge", 1);
        }
        else if (_pauseState == 0 && _state == ubt_task_state_running)
        {
            _SendMsg(0, "request_recharge", 0);
        }

        _pTaskCb->NotifyTaskStateAndStage(GetState(), _stage, 0, "");
    }
}

void CTaskAdjust::Sub_V_Recharge_Callback(const std_msgs::msg::Int64 ::SharedPtr msg)
{
    /**
     * 回充状态码
    enum class ResultCode {
        RECHARGE_FINISHED = 7000,        //  上桩完成
        RECHARGE_INIT_FAILED = 7001,     // 初始化失败
        RECHARGE_NOPILE = 7002,          // 没有检测到充电桩
        RECHARGE_TIMEOUT = 7003,         // 超时
        RECHARGE_TO_PILE_FAILED = 7004,  // 上桩失败
        RECHARGE_UNKNOWN_FAILED = 7100,// 其他失败
        RECHARGE_GO_OFF_FINISHED = 7010,  // 下桩完成
        RECHARGE_GO_OFF_FAILED = 7011,    // 下桩失败
    };
 topic:/recharge_result，data:1000  //发布开始回充指令接收成功
    */
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] Sub_V_Recharge_Callback data: %d", msg->data);
    int resultKey = msg->data;
    int closeReason = -1;
    std::string resultReasonStr = "";
    std::string closeReasonStr = "";

    if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed) || _state == ubt_task_state_pause)
    {
        return;
    }

    if (msg->data < 7000)  // is 1000 (RECHARGE_COMMAND_ACCEPTED) : retutn 
    {
        if (_state == ubt_task_state_stopping)
        {
            closeReason = ubt_task_result_adjust_base;
            resultReasonStr = "recv stoprsp";
            _SetState(ubt_task_state_closed, closeReason, resultReasonStr);
            _pTaskCb->NotifyTaskStateAndStage(_state, _stage, closeReason, "");
        }
        return ;
    }

    if (rechargeResultCodeMap.find(resultKey) == rechargeResultCodeMap.end())
    {
        RCLCPP_WARN(node->get_logger(),"[CTaskAdjust] Sub_V_Recharge_Callback : rechargeResultCodeMap is not key of %d", resultKey);
        return ;
    }

    // ok
    if (msg->data == 7000)
    {
        closeReason = 0;
    }
    // 失败
    else if (msg->data == 7001 || msg->data == 7002 || msg->data == 7003 || msg->data == 7004 || msg->data == 7100)
    {
        if (_stopReason != 0)
        {
            closeReason = _stopReason;
        }
        else
        {
            closeReason = msg->data;
        }
    }
    
    resultReasonStr = rechargeResultCodeMap[resultKey];

    if (closeReason >= 0)
    {
        _SetState(ubt_task_state_closed, closeReason, resultReasonStr);
        _pTaskCb->NotifyTaskStateAndStage(_state, _stage, closeReason, "");
    }
}

void CTaskAdjust::Sub_Adjust_Callback(const ubt_interface::msg::UbtAdjustMsg::SharedPtr msg)
{
    
    RCLCPP_INFO_STREAM(node->get_logger(),"taskId= " << _taskId << " state= " << _state << " msg= " << msg);
    if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
    {
        return;
    }
    int ires, closeReason = -1;
    std::string failDesc, msgBody = msg->body;
    ubt_proto_adjust::ubt_adjust_msg_com_rsp comRsp;
    char stateReasonBuf[128];
    stateReasonBuf[0] = 0;
    if (msg->head.msg_type == ubt_proto_adjust::ubt_adjust_msg_type_start_rsp)
    {
        ires = ubt_proto_adjust::ubtAdjustDecodeComRsp(msgBody, &comRsp, failDesc);
        if (comRsp.result < 0)
        {
            closeReason = ubt_task_result_adjust_startfail;
            sprintf(stateReasonBuf, "recv startrsp , result= %d is fail", comRsp.result);
        }
    }
    else if (msg->head.msg_type == ubt_proto_adjust::ubt_adjust_msg_type_stop_rsp)
    {
        ires = ubt_proto_adjust::ubtAdjustDecodeComRsp(msgBody, &comRsp, failDesc);
        if (_state == ubt_task_state_stopping)
        {
            closeReason = ubt_task_result_adjust_base;
            sprintf(stateReasonBuf, "recv stoprsp ,result= %d", comRsp.result);
        }
    }
    else if (msg->head.msg_type == ubt_proto_adjust::ubt_adjust_msg_type_status_nty)
    {
        ubt_proto_adjust::ubt_adjust_status statusNty;
        ires = ubt_proto_adjust::ubtAdjustDecodeStatus(msgBody, &statusNty, failDesc);
        if (statusNty.taskId != _taskId)
        {
            return;
        }
        if (statusNty.runStatus > 0)
        {
            return;
        }
        sprintf(stateReasonBuf, "recv status ,runStatus= %d ", statusNty.runStatus);
        if (statusNty.runStatus == 0)
        {
            closeReason = 0;
        }
        else
        {
            if (_stopReason != 0)
            {
                closeReason = _stopReason;
            }
            else
            {
                closeReason = ubt_task_result_adjust_base + (statusNty.runStatus * -1);
            }
        }
    }
    else
    {
    }
    if (closeReason >= 0)
    {
        _SetState(ubt_task_state_closed, closeReason, std::string(stateReasonBuf));
        _pTaskCb->NotifyTaskStateAndStage(_state, _stage, closeReason, "");
    }
    
}

int CTaskAdjust::_SendMsg(int msgType, void *pvMsg)
{
    
    RCLCPP_INFO(node->get_logger(),"msgType: %d",msgType);
    int ires;
    std::string failDesc;
    ubt_interface::msg::UbtAdjustMsg msg;
    msg.head.proto_id = ubt_com::ubt_proto_type_adjust;
    msg.head.msg_type = msgType;
    msg.head.msg_id = _msgId++;
    msg.head.msg_time = ubt_com::GetNowInMsecs();
    switch (msgType)
    {
        case ubt_proto_adjust::ubt_adjust_msg_type_start_req:
            ires = ubt_proto_adjust::ubtAdjustEncodeStartReq(msg.body, (ubt_proto_adjust::ubt_adjust_msg_start_req *)pvMsg, failDesc);
            break;
        case ubt_proto_adjust::ubt_adjust_msg_type_stop_req:
            ires = ubt_proto_adjust::ubtAdjustEncodeStopReq(msg.body, (ubt_proto_adjust::ubt_adjust_msg_stop_req *)pvMsg, failDesc);
            break;
        case ubt_proto_adjust::ubt_adjust_msg_type_pause_req:
        case ubt_proto_adjust::ubt_adjust_msg_type_continue_req:
        case ubt_proto_adjust::ubt_adjust_msg_type_query_req:
            ires = 0;
            break;
        default:
            failDesc = "unknown msgType";
            ires = -1;
    }
    if (0 == ires)
    {
        RCLCPP_INFO_STREAM(node->get_logger(),"_taskId: " << _taskId.c_str() << " state: " << _state << " msg: " << msg.head.msg_id);
        g_clsData._pub_adjust->publish(msg);
    }
    else
    {
        RCLCPP_WARN(node->get_logger(),"fail, taskId=%s msgType=%d failDesc=%s",
                 _taskId.c_str(), msgType, failDesc.c_str());
    }
    
    return ires;
}

bool CTaskAdjust::_SendMsg(int mode, std::string title, int sessionId)
{
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] _SendMsg: Send Recharge: mode: %d, title: %s, session_id: %d", mode, title.c_str(), sessionId);
    // json to str:
    // cJSON *msgJson = cJSON_CreateObject();
    // cJSON *dataObj = cJSON_CreateObject();
    cJSON *contentObj = cJSON_CreateObject();
    cJSON *modeObj = cJSON_CreateObject();

    cJSON_AddNumberToObject(modeObj, "mode", mode);
    cJSON_AddItemToObject(contentObj, "content", modeObj);
    cJSON_AddStringToObject(contentObj, "title", title.c_str());
    cJSON_AddNumberToObject(contentObj, "session_id", sessionId);
    // cJSON_AddItemToObject(dataObj, "data", contentObj);

    // std::string msgStr(cJSON_Print(contentObj));
    std::string msgStr(cJSON_PrintUnformatted(contentObj));

    std_msgs::msg::String msg;
    msg.data = msgStr;
    RCLCPP_INFO(node->get_logger(),"[CTaskAdjust] _SendMsg: recharge publish: %s", msg.data.c_str());
    g_clsData._pub_v_recharge->publish(msg);
    return true;
}

}



